#include "mat_pq.h"
#include "gauss_banda.h"
#include "utils.h"
#include <cassert>
#include <iostream>
#include <cmath>

//sin pivoteo! 
void gaussElim(mat& a, int p, int q) {
	int n=a.size();
	assert(n>0 && p<=n-1 && q<=n-1);
	forn(i,n) assert(int(a[i].size())==p+q+2);
	forn(i,n) {
		int k=min(i+p+1,n);
		int l=min(i+q+1,n);
		forsn(ni,i+1,k) {
			if (abs(getPQ(a,p,q,i,i)<1e-8)) {
				cout << "GAUSS PANIC" << endl;
				return;
			}
			double mul=getPQ(a,p,q,ni,i)/getPQ(a,p,q,i,i);
			forsn(j,i+1,l) getPQ(a,p,q,ni,j)-=getPQ(a,p,q,i,j)*mul;
			a[ni][p+q+1]-=a[i][p+q+1]*mul;
			getPQ(a,p,q,ni,i)=0.0;
		}
		
		//showPQ(a,p,q); cout << endl;
	}
}

vector<double> solvep(mat& a, int p, int q) {
	gaussElim(a,p,q);
	int n=a.size(); assert(n>0);
	forn(i,n) assert(int(a[i].size())==p+q+2);
	vector<double> res(n,0.0);
	dforn(i,n) {
		res[i]=a[i][p+q+1]; //columna de resultado
		int l=min(i+q+1,n);
		forsn(j,i+1,l) res[i]-=getPQ(a,p,q,i,j)*res[j];
		res[i]/=getPQ(a,p,q,i,i);
	}
	return res;
}

vector<double> solve(const mat& m, int p, int q) {
	mat a(m);
	return solvep(a,p,q);
}
